/*********************************************
封装小车编码电机运动部分. 
包含四麦克纳姆轮小车前进x米、后退x米、左转x米、右转x米、停止
小车四麦克纳姆轮初始化
**********************************************/

#define motor_loop_delay 10 //编码电机更新位置延时变量，单位毫秒

//麦克纳姆四轮小车四个电机引脚初始化
void car_init(){
  motor1.init();
  motor2.init();
  motor3.init();
  motor4.init();
}

//实时更新并保持小车四个电机的位置
void car_update(){
  motor1.update(); //时刻保持编码电机1的位置
  motor2.update(); //时刻保持编码电机2的位置
  motor3.update(); //时刻保持编码电机3的位置
  motor4.update(); //时刻保持编码电机4的位置
  delay(motor_loop_delay);
}

//小车四个轮子停止转动
void car_stop(){
  motor1.set_pwm(0);
  motor2.set_pwm(0);
  motor3.set_pwm(0);
  motor4.set_pwm(0);
}

/**************************************
小车运动状态，及具体运动距离函数
mode：表示小车运动状态
distance：表示小车在该状态运动的距离/角度
***************************************/
void car_move(int mode, float distance){
  switch(mode){
    case car_msg.forward:   {  car_move_distance(distance, 0.0, 0.0);  }break;
    case car_msg.back:      {  car_move_distance(-distance, 0.0, 0.0);  }break;
    case car_msg.turnleft:  {  car_move_distance(0.0, 0.0, distance);  }break;
    case car_msg.turnright: {  car_move_distance(0.0, 0.0, -distance);  }break;
    case car_msg.transleft: {  car_move_distance(0.0, distance, 0.0);  }break;
    case car_msg.transright:{  car_move_distance(0.0, -distance, 0.0);  }break;
    case car_msg.stop:      {  car_move_distance(0.0, 0.0, 0.0);       }break;
    default:                {  car_move_distance(0.0, 0.0, 0.0);       }break;
  }
}

void car_pwm(int mode, int pwm1, int pwm2, int pwm3, int pwm4){
  switch(mode){
    case car_msg.forward:   {  car_move_pwm(pwm1, pwm2, pwm3, pwm4);  }break;
    case car_msg.back:      {  car_move_pwm(pwm1, pwm2, pwm3, pwm4);  }break;
    case car_msg.turnleft:  {  car_move_pwm(pwm1, pwm2, pwm3, pwm4);  }break;
    case car_msg.turnright: {  car_move_pwm(pwm1, pwm2, pwm3, pwm4);  }break;
    case car_msg.transleft: {  car_move_pwm(pwm1, pwm2, pwm3, pwm4);  }break;
    case car_msg.transright:{  car_move_pwm(pwm1, pwm2, pwm3, pwm4);  }break;
    case car_msg.stop:      {  car_move_pwm(pwm1, pwm2, pwm3, pwm4);  }break;
  }
}

void car_move_pwm(int pwm1, int pwm2, int pwm3, int pwm4){
  motor1.set_pwm(pwm1);
  motor2.set_pwm(pwm2);
  motor3.set_pwm(pwm3);
  motor4.set_pwm(pwm4);
}

/*
功能：小车移动底层函数
*/
void car_move_distance(float dx, float dy, float dw){
  float distance_lf, distance_rf, distance_lb, distance_rb;
  distance_lf = -dx*0.8660 + dy*0.8660 + (dw*0.294*3.1415/180.0)*0.58;
  distance_rf =  dx*0.8660 + dy*0.8660 + (dw*0.294*3.1415/180.0)*0.58;
  distance_lb = -dx*0.8660 - dy*0.8660 + (dw*0.294*3.1415/180.0)*0.58;
  distance_rb =  dx*0.8660 - dy*0.8660 + (dw*0.294*3.1415/180.0)*0.58;
  wheel_move_distance(distance_lf, distance_rf, distance_lb, distance_rb);
}

//麦克纳姆小车四轮分别需要转动到的位置底层函数
void wheel_move_distance(float wheel_lf, float wheel_rf, float wheel_lb, float wheel_rb){
  motor1.set_pos(wheel_lf);
  motor2.set_pos(wheel_rf);
  motor3.set_pos(wheel_lb);
  motor4.set_pos(wheel_rb);

  while( (motor1.pos_flag) || motor2.pos_flag || motor3.pos_flag || motor4.pos_flag){
    if(motor1.pos_flag){motor1.pos_loop();}
    if(motor2.pos_flag){motor2.pos_loop();}
    if(motor3.pos_flag){motor3.pos_loop();}
    if(motor4.pos_flag){motor4.pos_loop();}
    // motor1.pos_loop();
    // motor2.pos_loop();
    // motor3.pos_loop();
    // motor4.pos_loop();
    delay(motor_loop_delay);
  }
  for(int i=0;i<100;i++){
    car_update();
  }
}

//小车动作测试
void car_action_test(){
  car_move_distance( 0.2, 0.0, 0.0);   //小车前进20厘米
  car_move_distance(-0.2, 0.0, 0.0);   //小车后退20厘米
  car_move_distance(0.0,  0.2, 0.0);   //小车左平移20厘米
  car_move_distance(0.0, -0.2, 0.0);   //小车右平移20厘米
  car_move_distance(0.0,  0.0, 90.0);  //小车左转90度
  car_move_distance(0.0,  0.0, -90.0); //小车右转90度
}


int motor_speed_now = 255;
void car_move_pwm_test(){
  car_pwm(car_msg.turnleft, -motor_speed_now, motor_speed_now, -motor_speed_now, motor_speed_now);
}
